package behavior;

import java.util.ArrayList;
import java.util.List;

import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.navigation.TachoPilot;

/**
 * An object of this class lets the robot drive a list. This list contains
 * integers representing different types of panels.
 * 
 * @author Simon & Maarten
 * 
 */
public class AutoPilot {
	private List<Integer> route;
	private TachoPilot pilot;
	private int dist = 10;
	private UltrasonicSensor sonicSensor = new UltrasonicSensor(SensorPort.S2);
	private Motor sensorMotor = Motor.A;
	private float adjustAngleDistance = 10;
	private int currentAdjustIndex = 2;
	private int adjustIndex = 0;

	public AutoPilot(List<Integer> route, TachoPilot pilot, int adjustIndex) {
		this.route = route;
		this.pilot = pilot;
		this.adjustIndex = adjustIndex;
	}

	/**
	 * This method takes the list with the route and lets the robot drive it.
	 * When the robot is supposed to drive a straight forward panel, we apply an
	 * algorithm to make sure that the robot is in the middle of the panel, at
	 * the right angle.
	 * 
	 */
	public void travel() {
		pilot.setSpeed(500);
		while (true) {
			for (Integer road : route) {
				if (road == 4 || road == 7 || road == 8)
					road = 2;
				switch (road) {
				case 1:
					pilot.travel(80);
					break;
				case 2:
					pilot.travel(80);
					break;
				case 6:
					if (wantToAdjust(6)) {
						sensorMotor.resetTachoCount();
						straighten();
						getInMiddle();
						sensorMotor.rotate(-sensorMotor.getTachoCount());
					}
					pilot.rotate(-90);
					pilot.travel(80);
					break;
				case 3:
					if (wantToAdjust(3)) {
						sensorMotor.resetTachoCount();
						straighten();
						getInMiddle();
						sensorMotor.rotate(-sensorMotor.getTachoCount());
					}
					pilot.rotate(90);
					pilot.travel(80);
					break;
				}

			}
		}

	}

	private boolean wantToAdjust(int panel) {
		
		if(currentAdjustIndex >= adjustIndex)
		{
			if(panel != 7 && panel != 4 && panel != 2 && panel != 8 &&panel!=1)
			{
				currentAdjustIndex = 0;
				return true;
			}
			
		}
		currentAdjustIndex ++;
		return false;
	}

	/**
	 * This method returns the position of the robot. The return value is an
	 * integer.
	 * 
	 * @return the distance from the middle of the panel (left from the middle
	 *         is negative)
	 */
	// private int getDistanceFromMiddle(){
	// Motor sensorMotor = Motor.A;
	// sensorMotor.setSpeed(180);
	// sensorMotor.rotate(90);
	// int distanceLeft= sonicSensor.getDistance();
	// sleep(100);
	// distanceLeft=sonicSensor.getDistance();
	// System.out.println(distanceLeft);
	// sensorMotor.rotate(-90);
	// return -40+distanceLeft;
	// }
	public void getInMiddle() {
		sensorMotor.resetTachoCount();
		sonicSensor.getDistance(); // Hurr Ima sonicsensor Durr
		sleep(1000);
		ArrayList<Integer> distances = new ArrayList<Integer>();
		// getSonicSensor().getDistance()
		sensorMotor.rotate(90);
		sleep(80);
		distances.add(readValue());
		sensorMotor.rotate(-90);
		sleep(80);
		distances.add(readValue());
		sensorMotor.rotate(-90);
		sleep(80);
		distances.add(readValue());
		sensorMotor.rotate(90);

		int left = distances.get(0);
		int right = distances.get(2);
		if (left < 40) {
			// float angle = getAngle(left,true);
			// pilot.rotate((float)(angle*180/Math.PI));
			sensorMotor.rotate(90);
			left = readValue();
			sensorMotor.rotate(-90);
			pilot.rotate(90);
			pilot.travel(-(37 - left));
			pilot.rotate(-90);
		} else if (right < 40) {
			// float angle = getAngle(right, false);
			// pilot.rotate(angle);
			// pilot.rotate(-(float)(angle*180/Math.PI));
			sensorMotor.rotate(-90);
			right = readValue();
			sensorMotor.rotate(90);
			pilot.rotate(-90);
			pilot.travel(-(37 - right));
			pilot.rotate(90);
		} else if (left > 40 && left < 70) {
			// float angle = getAngle(left,true);
			// pilot.rotate((float)(angle*180/Math.PI));
			sensorMotor.rotate(90);
			left = readValue();
			sensorMotor.rotate(-90);
			pilot.rotate(-90);
			pilot.travel(-(left - 37));
			pilot.rotate(90);
		} else if (right > 40 && right < 70) {
			// float angle = getAngle(right, false);
			// pilot.rotate(-(float)(angle*180/Math.PI));
			sensorMotor.rotate(90);
			right = readValue();
			sensorMotor.rotate(-90);
			pilot.rotate(90);
			pilot.travel(-(-37 + right));
			pilot.rotate(-90);
		}
		sleep(80);
		int forward = readValue();
		if (forward > 40 && forward < 70) {
			pilot.travel(forward - 37);
		} else if (forward < 40) {
			pilot.travel(-(37 - forward));
		}
		sensorMotor.rotate(-sensorMotor.getTachoCount());

	}

	private void sleep(int mili) {
		try {
			Thread.sleep(mili);
		} catch (InterruptedException e) {

		}
	}

	// private float getAngle(){
	// //int angle = 0;
	// Motor sensorMotor = Motor.A;
	// sensorMotor.setSpeed(180);
	// sensorMotor.rotate(90);
	// int distance1= readValue();
	// LCD.drawString("dist1:" + distance1,0,0);
	// pilot.travel(dist);
	// int distance2= readValue();
	// LCD.drawString("dist2:" + distance2,0,1);
	// sensorMotor.rotate(-90);
	// pilot.travel(-dist);
	// double v = Math.abs(distance1-distance2);
	// float angle = (float) Math.atan(v/dist); //Nog in radians
	// angle *= 180/Math.PI;
	// if(distance1 > distance2){
	// angle *= -1;
	// }
	// return angle; //In graden
	// }
	public void straighten() {
		sensorMotor.resetTachoCount();
		sensorMotor.rotate(90);
		int left = sonicSensor.getDistance();
		sleep(80);
		left = sonicSensor.getDistance();
		sensorMotor.rotate(-180);
		int right = sonicSensor.getDistance();
		sensorMotor.rotate(90);

		if (left < right) {
			float angle = getAngle(true);
			pilot.rotate(angle);
		} else {
			float angle = getAngle(false);
			pilot.rotate(angle);
		}
		sensorMotor.rotate(-sensorMotor.getTachoCount());
	}

	private float getAngle(boolean left) {
		int sign = left ? 1 : -1;
		LCD.drawString("sign:" + sign, 0, 4);
		sensorMotor.rotate(sign * 90);
		int distance1 = readValue();
		LCD.drawString("dist1:" + distance1, 0, 0);
		pilot.travel(adjustAngleDistance);
		int distance2 = readValue();
		LCD.drawString("dist2:" + distance2, 0, 1);
		sensorMotor.rotate(sign * -90);
		pilot.travel(-adjustAngleDistance);
		double v = Math.abs(distance1 - distance2);
		float angle = (float) Math.atan(v / adjustAngleDistance); // Nog in
		// radians
		angle *= 180 / Math.PI;
		if (distance1 > distance2) {
			angle *= -1;
		}
		return sign * angle; // In graden
	}

	public int readValue() {
		ArrayList<Integer> vals = new ArrayList<Integer>();
		for (int i = 0; i < 100; i++) {
			vals.add(sonicSensor.getDistance());
		}
		int finalValue = vals.get(0);
		for (int value : vals) {
			if (value != 255)
				finalValue = value;
		}
		return finalValue;
	}
}
